#include "TDOA.h"

double xMAX;// 搜索变量范围
double xMIN;
double yMAX;
double yMIN;
double DistanceOfMachMH;//马赫数距离

TDOA::TDOA(QObject *parent)
    : QObject{parent}
{
  if(A2X == 0){
      A2X = 0.393;
    }
  if(A3X == 0){
      A3X = 0.787;
    }
}

QVector<double> TDOA::CoordinateProcessing(const QVector<uint16_t> &data)
{
    xMAX = HorizonMin + 1; // 假设 HorizonMin 为全局变量
    xMIN = HorizonMin;    // 假设 HorizonMin 为全局变量
    yMAX = VerticalMin + 1; // 假设 VerticalMin 为全局变量
    yMIN = VerticalMin;    // 假设 VerticalMin 为全局变量
    DistanceOfMachMH = DistanceOfMach; // 假设 DistanceOfMach 为全局变量

    double a, b;
    QVector<int32_t> differenceData;
    uint16_t base = data[0]; //第一个数据为中间基站数据，所以这里采用第一个作为基点
    differenceData.push_back(0); // base-base
    differenceData.push_back(compareData(base, data[1])); // 左-base
    differenceData.push_back(compareData(base, data[2])); // 右-base
    differenceData.push_back(compareData(base, data[3])); // 前-base

    if (!judgeTimePoint(differenceData)) {
        throw runtime_error("时间差错误");
    }

    QVector<QVector<double>> A3{{A1X, A1Y}, {A2X, A2Y}, {A3X, A3Y}};
    positionCalculate(differenceData, A3, &a, &b);
    QVector<double> res;
    res.push_back(a);
    res.push_back(b);
    return res;
}

double TDOA::positionCalculate(QVector<int32_t> deltaT, QVector<QVector<double> > A3, double *BestX, double *BestY)
{
    //以时间戳作为种子生成随机数，确保每次生成的随机数序列不同
    QRandomGenerator generator(QDateTime::currentMSecsSinceEpoch());
    //生成[0,1]的随机数
    auto RANDTO1 = [&generator]() {
        return static_cast<double>(generator.generate()) / generator.max();
    };

    int16_t i, p;
    double NextX, NextY;
    double changer;
    double p1;

    *BestX = 0.4;  // 初始值和PreX需要相同
    *BestY = 0.8;  // 同上

    double T = 1.0; // 初始温度

    /***********初值设定********/
    double PreX = 0.4;
    double PreY = 0.8;  // 趋于十环的坐标点

    // 如果误差小于容差或者温度小于最低值了
    while (true) {
        T = T * K;  // 降温
        // 循环马尔可夫链长度
        for (i = 0; i < L; i++) {
            p = 0;
            // 直到产生一个新解
            while (p == 0) {
                NextX = PreX + S * (RANDTO1() - 0.5) * (xMAX - xMIN);  // 扰动，产生一个新解
                NextY = PreY + S * (RANDTO1() - 0.5) * (yMAX - yMIN);
                // 如果在搜索范围
                if ((NextX >= xMIN) && (NextX <= xMAX) && (NextY >= yMIN) && (NextY <= yMAX)) {
                    p = 1;
                    //break;
                }
            }

            // 判断最新解与最优解的误差
            if (optimizationFunction(*BestX, *BestY, deltaT, A3) > optimizationFunction(NextX, NextY, deltaT, A3)) {
                // 此为新的最优解
                *BestX = NextX;
                *BestY = NextY;
            }
            // 如果上一次解误差大于新解，则接受新解，这里是个坑，如果写-号u类型不支持
            if (optimizationFunction(PreX, PreY, deltaT, A3) > optimizationFunction(NextX, NextY, deltaT, A3)) {
                // 接受新解
                PreX = NextX;
                PreY = NextY;
            } else {
                changer = ((-1) * (optimizationFunction(NextX, NextY, deltaT, A3) - optimizationFunction(PreX, PreY, deltaT, A3))) / T;
                // 计算概率
                p1 = qExp(changer);
                // 以一定概率接受较差的解
                if (p1 > RANDTO1()) {
                    PreX = NextX;
                    PreY = NextY;
                }
            }
        }
        if (T < 0.000001) {
            break;
        }
    }
    return 0.0;
}

double TDOA::optimizationFunction(double x, double y, const QVector<int32_t> &deltaT, const QVector<QVector<double> > &A3)
{
    double vH;
    auto dleTime = std::abs(static_cast<double>(deltaT[3]));
    auto speedOfBullet = DistanceOfMachMH * 1000000 / dleTime;
    auto theta = std::asin((SoundSpeed * dleTime) / (DistanceOfMachMH * 1000000));
    vH = speedOfBullet * std::tan(theta);

    if (deltaT[3] >= 0) {
        vH = SoundSpeed;
    }

    double r1, r2;
    double f21, f01;
    double value;

    r1 = static_cast<double>(deltaT[1]) * vH * precision;
    r2 = static_cast<double>(deltaT[2]) * vH * precision;
    double baseDifference = std::sqrt(std::pow(A3[1][0] - x, 2) + std::pow(A3[1][1] - y, 2));

    f01 = -r1 + (std::sqrt(std::pow(A3[0][0] - x, 2) + std::pow(A3[0][1] - y, 2)) - baseDifference);
    f21 = -r2 + (std::sqrt(std::pow(A3[2][0] - x, 2) + std::pow(A3[2][1] - y, 2)) - baseDifference);

    value = std::pow(f21, 2) + std::pow(f01, 2);
    return value;
}

bool TDOA::judgeTimePoint(QVector<int32_t> differenceData)
{
    foreach (int32_t value, differenceData) {
        if(value >= -1200 && value <= 1200)
            return true;
    }
    return false;
}

// compareData 溢出处理方法,判断数据是否溢出.如溢出,处理数据.
int32_t TDOA::compareData(uint16_t base, uint16_t another) {
    int32_t res = static_cast<int32_t>(another) - static_cast<int32_t>(base);
    if (abs(res) > 30000) {
        res = static_cast<int32_t>(UINT16_MAX) - static_cast<uint16_t>(std::abs(res)) + 1;
    }
    return res;
}
